﻿/*
 * HCC Robotica 
 * Workshop Robot programmeren met C# 
 * Oefening : wo_07	Robot, een vierkantje rijden 
 * Naam     : Hinnie
 * 
 */

using System;
using System.Threading;
using System.IO.Ports;
using System.Text;

using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

using GHIElectronics.NETMF.Hardware;
using GHIElectronics.NETMF.FEZ;

namespace wo_07
{
    public class wo_07
    {
        public static void Main()
        {
            OutputPort LED = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.LED, false);
            InputPort ButtonLDR = new InputPort((Cpu.Pin)FEZ_Pin.Digital.LDR, false, Port.ResistorMode.PullUp);
            bool ledState = false;
            bool ledSwitch = false;
            const int loopTime = 5;
            const int flashTime = 200 / loopTime;
            const int turnTime = 100 / loopTime;
            const int straightTime = 1000 / loopTime;
            const int stopTime = 20 / loopTime;
            const sbyte leftSpeed = 75;
            const sbyte rightSpeed = 75;
            while (true)
            {
                bool butLDR = true;
                for (int i = 0; i < 2; i++)
                {
                    FEZMini_Robot.Beep(8000, 50);
                    Thread.Sleep(100);
                }
                while (ButtonLDR.Read() == butLDR)
                {
                    ledSwitch = !ledSwitch;
                    LED.Write(ledSwitch);           // less intensity onboard LED
                    Thread.Sleep(loopTime);
                }
                FEZMini_Robot.Beep(8000, 100);
                while (ButtonLDR.Read() != butLDR) { }

                int stepTimeCount = 0;
                int flashCount = 0;
                int stepNr = 0;
                int lastStepNr = -1;
                bool firstEntry;
                while (true)
                {
                    if (lastStepNr != stepNr)
                    {
                        lastStepNr = stepNr;
                        firstEntry = true;
                        FEZMini_Robot.Beep(8000, 50);
                    }
                    else
                        firstEntry = false;

                    if ((stepNr % 2) == 0)      // drive straight   
                    {
                        if (firstEntry)
                        {
                            FEZMini_Robot.Move(leftSpeed, rightSpeed);
                            stepTimeCount = 0;
                        }
                        else
                        {
                            if (stepTimeCount < straightTime)
                                stepTimeCount++;
                            else
                                stepNr++;
                        }
                    }
                    else                        // 90 graden bocht   
                    {
                        if (firstEntry)
                        {
                            firstEntry = false;
                            FEZMini_Robot.Move(leftSpeed, -rightSpeed);
                            stepTimeCount = 0;
                        }
                        else
                        {
                            if (stepTimeCount < turnTime)
                                stepTimeCount++;
                            else
                            {
                                stepNr++;
                                FEZMini_Robot.Move(-20, +20);
                                Thread.Sleep(stopTime);
                                FEZMini_Robot.Move(0, 0);
                                Thread.Sleep(stopTime);
                            }
                        }
                    }

                    if (stepNr >= 8)
                    {
                        FEZMini_Robot.Move(-20, -20);
                        Thread.Sleep(stopTime);
                        FEZMini_Robot.Move(0, 0);
                        break;
                    }

                    if (flashCount++ >= flashTime)
                    {
                        flashCount = 0;
                        ledState = !ledState;       // toggle LED state
                        LED.Write(ledState);        // blink onboard LED
                    }
                    if (ledState)
                    {
                        ledSwitch = !ledSwitch;
                        LED.Write(ledSwitch);       // less intensity onboard LED
                    }
                    Thread.Sleep(loopTime);
                }
            }
        }
    }
}
